#include "WPILib.h"
#include "Robot2489.h"

Robot2489::Robot2489(void) {
	InitJaguar();
	//InitEncoder();
	InitJoystick();
	InitLightSensor();
	//InitCamera();
	InitSpike();
	
	//manipulator1 = new Joystick (M1P);
	//manipulator2 = new Joystick (M2P);
	screen = DriverStationLCD::GetInstance();
	ds = DriverStation::GetInstance();
	dseio = &ds->GetEnhancedIO();
	
	timer = new Timer();
	
	//deployer = new Servo(4,1);
	//deployer->Set(0.0);
	
	
	myRobot = new RobotDrive(leftFrontMotor,
			leftBackMotor,
			rightFrontMotor,
			rightBackMotor); // these must be initialized in the same order
	
	myRobot->SetExpiration(0.1);
}

void Robot2489 :: InitSpike()
{
	leftSpike = new Relay(LEFTSIDECAR,7);
	rightSpike = new Relay(RIGHTSIDECAR,7);
	//armSpike = new Relay(LEFTSIDECAR, 4);
	//topWheelSpike = new Relay(RIGHTSIDECAR,1);
	//bottomWheelSpike = new Relay(RIGHTSIDECAR, BOTTOMSPINNINGWHEEL);
}
void Robot2489 :: InitJaguar()
{
	leftFrontMotor = new Jaguar(LEFTSIDECAR, LEFTFRONT);
	leftBackMotor = new Jaguar(LEFTSIDECAR, LEFTBACK);
	rightFrontMotor = new Jaguar(RIGHTSIDECAR, RIGHTFRONT);
	rightBackMotor = new Jaguar(RIGHTSIDECAR, RIGHTBACK);
	//topWheel = new Jaguar(RIGHTSIDECAR,1);
	//bottomWheel = new Jaguar(RIGHTSIDECAR, BOTTOMSPINNINGWHEEL);
	armMotor = new Jaguar(LEFTSIDECAR, 4);
}

//Commented until potentiometer is wired up
/*
void Robot2489 :: InitAnalogChannel()
{
	potentiometer = new AnalogChannel(1, 1);
	startAngle = potentiometer->GetValue();
}
*/

void Robot2489 :: InitEncoder()
{
	// Setup for the left front pid encoder
	leftFrontEncoder = new Encoder(LFENC_A, LFENC_B);
	leftFrontEncoder->SetDistancePerPulse(distancePerPulse4X);
	
	//Setup for left back pid encoder
	leftBackEncoder = new Encoder(LBENC_A, LBENC_B);
	leftBackEncoder->SetDistancePerPulse(distancePerPulse4X);
	
	//Setup for right front pid encoder
	rightFrontEncoder = new Encoder(RFENC_A, RFENC_B);
	rightFrontEncoder->SetDistancePerPulse(distancePerPulse4X);
	
	//Setup for right back pid encoder
	rightBackEncoder = new Encoder(RBENC_A, RBENC_B);
	rightBackEncoder->SetDistancePerPulse(distancePerPulse4X);
}

void Robot2489 :: InitJoystick()
{
	// For use in MecanumDrive
	strafeStick = new Joystick(STRAFESTICKPORT); // as they are declared above.
	spinStick = new Joystick(SPINSTICKPORT);
	// For use in TankDrive
	rightStick = new Joystick(RIGHTSTICK); 
	leftStick = new Joystick(LEFTSTICK);
	// For videogame controllers
	game = new Joystick(GAMEPORT);
	//For manipulator.
	manipulator1 = new Joystick(MANIPSTICKPORT);
}

void Robot2489 :: InitLightSensor()
{
	rightLineSensor = new DigitalInput(LEFTSIDECAR, RIGHTLINESENSOR);
	middleLineSensor = new DigitalInput(LEFTSIDECAR, MIDDLELINESENSOR);
	leftLineSensor = new DigitalInput(LEFTSIDECAR, LEFTLINESENSOR);	
}
/*
void Robot2489 :: InitCamera()
{
	camera = &AxisCamera::GetInstance();
}*/

void Robot2489 :: InitLimitSwitch()
{
	lowOutSwitch = new DigitalInput(LOSWITCH);
	lowInSwitch = new DigitalInput(LISWITCH);
	feederSwitch = new DigitalInput(FEEDSWITCH);
	midOutSwitch = new DigitalInput(MOSWITCH);
	midInSwitch = new DigitalInput(MISWITCH);
	bottomLimit = new DigitalInput (BOTTOMLIMIT);
	topLimit = new DigitalInput (TOPLIMIT);
}
/*
void Robot2489 :: InitGyro()
{
	gyro = new Gyro(GYROSLOT, GYROCHANNEL);
	gyro->SetSensitivity(0.007);
}
*/
START_ROBOT_CLASS(Robot2489);
